/*!
  @file camera_params.h
  @brief 
  @author Kyohei Otsu <kyon@ac.jaxa.jp>
  @date April, 2013
*/

#ifndef _CAMERA_PARAMS_H_
#define _CAMERA_PARAMS_H_

#include <vector>

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>

#include <opencv2/core/core.hpp>


namespace vo
{

class CameraParams {
 public:
  typedef boost::shared_ptr<CameraParams> Ptr;
  typedef boost::shared_ptr<CameraParams const> ConstPtr;

  CameraParams() { 
    K.push_back(cv::Mat(3, 3, CV_32F));
    K.push_back(cv::Mat(3, 3, CV_32F));
    D.push_back(cv::Mat(1, 5, CV_32F));
    D.push_back(cv::Mat(1, 5, CV_32F));
    wT.push_back(cv::Mat::eye(4, 4, CV_32F));
    wT.push_back(cv::Mat::eye(4, 4, CV_32F));
    P.push_back(cv::Mat::eye(3, 4, CV_32F));
    P.push_back(cv::Mat::eye(3, 4, CV_32F));
  }

  ~CameraParams() {}

  //! read camera parameters from yaml file
  void initFromFile(std::string &yaml);

  void publishTF();

  tf::TransformBroadcaster tf_pub_;

  std::vector<cv::Mat> K;  //!< internal matrices
  std::vector<cv::Mat> D;  //!< distortion vectors
  cv::Size image_size;  //!< image_size;
  float baseline;       //!< baseline in m

  std::vector<cv::Mat> P;   //!< Projection matrices
  std::vector<cv::Mat> wT;  //!< Translation matrices from each camera to world

 private:
  void convert(const cv::Mat &T, tf::Transform &transform);
};

}  // namespace vo

#endif
